#!/usr/bin/env python2
# -*- coding: utf-8 -*-
''' 测试用，无情的发布机器，模拟动捕系统接口发布消息 '''
import rospy
import pandas as pd
import numpy as np
from math import *
from elfin_driver import TransMartrix2xyzrpy, xyzrpy2TransMartrix
from geometry_msgs.msg import PoseStamped

# x,y,z,r,p,y [mm]
DATA_PATH = 'C.csv'
TARGET_TO_PEN = [0, 0, 0, pi, 0, 0]

def fileRead():
    '''读入文件，转化为xyzrpy列表并返回'''
    pd_data = pd.read_csv(DATA_PATH)  # 读取存放的文件
    datas = pd_data.values  # 转化为np
    datas *= [1.0, 1.0, 1.0, pi/180.0, pi/180.0, pi/180.0]
    return datas

def transform(datas):
    global org_z
    '''原始靶标到笔尖的坐标变换'''
    datas_trans = []
    for data in datas:
        T_base2target = xyzrpy2TransMartrix(data)
        T_target2pen = xyzrpy2TransMartrix(TARGET_TO_PEN)
        T_base2pen = T_base2target.dot(T_target2pen)
        datas_trans.append(TransMartrix2xyzrpy(T_base2pen))  # 转换为基坐标系下笔尖的坐标列表
    return datas_trans

def _euler2quaternion(euler):
    '''欧拉角转四元数'''
    roll, pitch, yaw = euler
    cosroll = cos(roll * 0.5)
    sinroll = sin(roll * 0.5)
    cospitch = cos(pitch * 0.5)
    sinpitch = sin(pitch * 0.5)
    cosyaw = cos(yaw * 0.5)
    sinyaw = sin(yaw * 0.5)
    qw = cosroll * cosyaw * cospitch + sinroll * sinyaw * sinpitch
    qx = sinroll * cosyaw * cospitch - cosroll * sinyaw * sinpitch
    qy = cosroll * cosyaw * sinpitch + sinroll * sinyaw * cospitch
    qz = cosroll * sinyaw * cospitch - sinroll * cosyaw * sinpitch
    return (qw, qx, qy, qz)

def pub_point(pubber, point):
    '''单位[mm][rad]'''
    x = point[0]
    y = point[1]
    z = point[2]
    roll = point[3]
    pitch = point[4]
    yaw = point[5]
    ps = PoseStamped()
    ps.header.stamp = rospy.get_rostime()
    ps.header.frame_id = 'elfin_base_link'
    ps.pose.position.x = x
    ps.pose.position.y = y
    ps.pose.position.z = z
    qw, qx, qy, qz = _euler2quaternion([roll, pitch, yaw])
    ps.pose.orientation.x = qx
    ps.pose.orientation.y = qy
    ps.pose.orientation.z = qz
    ps.pose.orientation.w = qw
    pubber.publish(ps)

if __name__ == '__main__':
    rospy.init_node('record_pub_test', anonymous=True)
    pubber = rospy.Publisher('vrpn_client_node/SapphiArt/pose', PoseStamped, queue_size=1)
    datas = fileRead()
    datas_trans = transform(datas)
    rospy.sleep(0.5)  
    pub_point(pubber, datas_trans[0])
    rospy.sleep(3)  # 先发一帧告诉creator动捕系统正常工作
    datas_trans = datas_trans[40:]
    for data in datas_trans:
        pub_point(pubber, data)
        rospy.sleep(0.02)  # 50HZ